
use strict;
use warnings;

use Log::Handler;
use Getopt::Long;
use IO::Uncompress::AnyUncompress qw(anyuncompress $AnyUncompressError);
use LWP::Simple;
use XML::Simple;
use List::Util qw{ min max sum };
use DateTime::Format::RFC3339;

use Data::Dump 'dd';


##  Settings
my $timecoef    = 0.5;      # per year
# track segment matching    
my $maxtime     = 30;       # max segment time, in seconds
my $maxspeed    = 200;      # max segment speed, in km/h
my $maxdistance = 0.012;    # max segment from road distance, in km
my $maxangle    = 10;       # max angle between track and road, in degrees


##  Defaults
my $osmtracks = 1;
my $savegpx = 0;
my @gpxfiles;

GetOptions (
    'osm!'      => \$osmtracks,
    'gpx=s'     => sub{ push @gpxfiles, glob $_[1] },
    'savegpx!'  => \$savegpx,
);


my $wayid = $ARGV[0] // '9886757';
my $now = time;




my $log = Log::Handler->new();
$log->add(
    screen => {
        log_to      => "STDERR",
        #maxlevel    => "notice",
        maxlevel    => "info",
        #maxlevel    => "debug",
        timeformat  => "%H:%M:%S"
    }
);


##  Road segments

$log->notice( "Downloading way $wayid" );
my $wayxml = get "http://www.openstreetmap.org/api/0.6/way/$wayid/full";

my $waydata = XMLin $wayxml, ForceArray => 1, KeyAttr => [ 'id', 'k' ];
my @waypoints = map { [ @{ $waydata->{node}->{$_->{ref}} }{'lon', 'lat'} ] } 
                    @{ $waydata->{way}->{$wayid}->{nd} };
my @waysegs;

for my $i ( 1 .. $#waypoints ) {
    push @waysegs, [ @{ $waypoints[$i-1] }, @{ $waypoints[$i] } ];
}

$log->info( scalar( @waysegs ) . " segments" );


##  Bounding box

my @bbox = (
    ( min map { $waydata->{node}->{$_}->{lon} } keys %{ $waydata->{node} } ),
    ( min map { $waydata->{node}->{$_}->{lat} } keys %{ $waydata->{node} } ),
    ( max map { $waydata->{node}->{$_}->{lon} } keys %{ $waydata->{node} } ),
    ( max map { $waydata->{node}->{$_}->{lat} } keys %{ $waydata->{node} } ),
);

my $klon = cos( ( $bbox[1] + $bbox[3] ) / 2 / 180 * 3.1416 );

my $bdelta = 0.0003;
@bbox = ( $bbox[0]-$bdelta/$klon, $bbox[1]-$bdelta, $bbox[2]+$bdelta/$klon, $bbox[3]+$bdelta );

my $bbox = join q{,}, @bbox;
$log->info( "bbox: $bbox" );



##  Process available tracks

my $fseg = 0;
my $fdist = 0;
my $fwdist = 0;
my $fsdist = 0;

my $bseg = 0;
my $bdist = 0;
my $bwdist = 0;
my $bsdist = 0;

if ( $osmtracks ) {
    $log->notice( "Downloading OSM tracks" );
    
    my $gpxpage = 0;
    while ( 1 ) {
        my $gpx = get "http://api.openstreetmap.org/api/0.6/trackpoints?bbox=$bbox&page=$gpxpage";
        my $numpoints = scalar( my @points = $gpx =~ /<trkpt/g );
        $log->notice( "GPX page $gpxpage: $numpoints points" );

        process_gpx( $gpx );

        if ( $savegpx ) {
            open my $dump, '>:utf8', "$wayid.page$gpxpage.gpx";
            print $dump $gpx;
            close $dump;
        }
        #last;

        last if $numpoints < 5000;
        $gpxpage ++;
    }
}

for my $gpxfile ( @gpxfiles ) {
    $log->notice( "Loading file $gpxfile" );

    my $gpx;
    anyuncompress $gpxfile => \$gpx;
    process_gpx( $gpx );
}

printf "\n\n";
printf "Way $wayid: %f km\n\n",
    sum map { SegmentLength([ [ $_->[0]*$klon, $_->[1] ], [ $_->[2]*$klon, $_->[3] ] ]) * 40000 / 360 } @waysegs;

if ( $bdist+$fdist ) {
    printf "Forward:  %d segs, %f km, speed: %f\n",
        $fseg, $fdist, $fsdist/$fwdist
        if $fdist > 0;
    printf "Backward: %d segs, %f km, speed: %f\n",
        $bseg, $bdist, $bsdist/$bwdist
        if $bdist > 0;
    printf "Average speed total:  %f\n", 
        ($bsdist+$fsdist) / ($bwdist+$fwdist);

    my %tag = %{ $waydata->{way}->{$wayid}->{tag} };

    my $oneway;
    $oneway = 1     if exists $tag{junction} && $tag{junction} eq 'roundabout';
    $oneway = 1     if exists $tag{highway} && $tag{highway} eq 'motorway';
    $oneway = 1     if exists $tag{highway} && $tag{highway} eq 'motorway_link';
    $oneway = $tag{oneway} if exists $tag{oneway};
    undef $oneway   if defined $oneway && $oneway eq '0';
    undef $oneway   if defined $oneway && $oneway eq 'no';
    undef $oneway   if defined $oneway && $oneway eq 'false';

    if ( defined $oneway ) {
        printf "\nThis is oneway road, use %s speed!\n",
            ( $oneway eq '-1' ? 'backward' : 'forward' );
    }
}
else {
    print "No useful tracks on this way :(\n";
}



sub process_gpx {
    my ($gpx) = @_;
    my $gpxdata = XMLin $gpx, ForceArray => 1; # [ qw{ trk trkseg trkpt } ];
    for my $trk ( @{ $gpxdata->{trk} } ) {
        $log->info( sprintf "Track: %s", exists $trk->{name} ? $trk->{name}->[0] : 'unnamed' );
        for my $trkseg ( @{ $trk->{trkseg} } ) {
            process_track( 
                map {[ $_->{lon}, $_->{lat}, secs( $_->{time}->[0] ) ]} 
                grep { exists $_->{time} }
                @{ $trkseg->{trkpt} } 
            );
        }
    }
}

sub process_track {
    my $prev = undef;
    for my $pt ( @_ ) {
        if ( defined $prev ) {
            next if $prev->[0] == $pt->[0] && $prev->[1] == $pt->[1];
            process_track_seg( @$prev[0,1], @$pt[0,1], $pt->[2] - $prev->[2], $prev->[2] );
        }
        $prev = $pt;
    }
}


sub process_track_seg {

    my $tseg = [ [ shift() * $klon, shift ], [ shift() * $klon, shift ] ];
    my $mtseg = [ ($tseg->[0][0]+$tseg->[1][0])/2, ($tseg->[0][1]+$tseg->[1][1])/2 ];
    my $ti = shift;
    my $ttime = shift;

    #$log->debug( sprintf "tseg:  (%f, %f) - (%f, %f)", @{$tseg->[0]}, @{$tseg->[1]} );

    return if $ti < 1;
    return if $ti > $maxtime;

    my $length = SegmentLength( $tseg ) * 40000 / 360;
    my $speed  = $length / ($ti/3600);

    return if $speed > $maxspeed;

    my $along;
    my $cos;
    for my $w ( @waysegs ) {
        my $wseg = [ [ $w->[0]*$klon, $w->[1] ], [ $w->[2]*$klon, $w->[3] ] ];

        #$log->debug( sprintf "wseg:  (%f, %f) - (%f, %f)", @{$tseg->[0]}, @{$tseg->[1]} );

        my $dist = abs DistanceToSegment([ @$wseg, $mtseg ])*40000/360;
        
        #$log->debug( sprintf "dist: %f", $dist );

        next unless $dist < $maxdistance;

        $cos = angle_cos( $wseg, $tseg );
        next unless abs $cos > cos($maxangle/180*3.1416);

        $along = 1;
        last;
    }

    return unless $along;

    my $tcoef = $timecoef ** ( ($now-$ttime)/60/60/24/365.25 );

    $log->debug( sprintf "time: %f   length: %f   speed: %f  %s\n",
        $ti, $length, $speed, ( $cos > 0 ? 'forward' : 'backward' ) );
    $log->debug( sprintf "   tcoef:  %f", $tcoef );
    

    if ( $cos > 0 ) {
        $fseg ++;
        $fdist  += $length;
        $fwdist += $length * $tcoef;
        $fsdist += $speed * $length * $tcoef;
    }
    else {
        $bseg ++;
        $bdist  += $length;
        $bwdist += $length * $tcoef;
        $bsdist += $speed * $length * $tcoef;
    }
}



sub secs {
    return DateTime::Format::RFC3339->new()->parse_datetime( $_[0] )->epoch();
}

sub angle_cos {
    my ($s1, $s2) = @_;
    my $sq =  ( ($s1->[0][0] - $s1->[1][0])**2 + ($s1->[0][1] - $s1->[1][1])**2 )
            * ( ($s2->[0][0] - $s2->[1][0])**2 + ($s2->[0][1] - $s2->[1][1])**2 );

    return -1   if ( $sq == 0);
    
    my $cos = ( ($s1->[0][0] - $s1->[1][0]) * ($s2->[0][0] - $s2->[1][0])
              + ($s1->[0][1] - $s1->[1][1]) * ($s2->[0][1] - $s2->[1][1]) ) / sqrt( $sq );

    return $cos;
}

sub segment_length {
    my ($p1, $p2) = @_;
    return sqrt( ($p2->[0] - $p1->[0])**2 + ($p2->[1] - $p1->[1])**2 );
}




##  functions copied from Math::Geometry::Planar

use Carp;

sub SegmentLength {
  my $pointsref = $_[0];
  my @points = @$pointsref;
  if (@points != 2) {
    carp("Need 2 points for a segment length calculation");
    return;
  }
  my @a = @{$points[0]};
  my @b = @{$points[1]};
  my $length = sqrt(DotProduct([$points[0],$points[1],$points[0],$points[1]]));
  return $length;
}

sub DotProduct {
  my $pointsref = $_[0];
  my @points = @$pointsref;
  my (@p1,@p2,@p3,@p4);
  if (@points == 4) {
    @p1 = @{$points[0]};
    @p2 = @{$points[1]};
    @p3 = @{$points[2]};
    @p4 = @{$points[3]};
  } elsif (@points == 3) {
    @p1 = @{$points[0]};
    @p2 = @{$points[1]};
    @p3 = @{$points[0]};
    @p4 = @{$points[2]};
  } else {
    carp("Need 3 or 4 points for a dot product");
    return;
  }
  return ($p2[0]-$p1[0])*($p4[0]-$p3[0]) + ($p2[1]-$p1[1])*($p4[1]-$p3[1]);
}

sub DistanceToSegment {
  my $pointsref = $_[0];
  my @points = @$pointsref;
  if (@points < 3) {
    carp("DistanceToSegment needs 3 points defining a segment and a point");
    return;
  }
  # the perpendicular distance is the height of the parallelogram defined
  # by the 3 points devided by the base
  # Note the this is a signed value so it can be used to check at which
  # side the point is located
  # we use dot products to find out where point is located1G/dotpro
  my $d1 = DotProduct([$points[0],$points[1],$points[0],$points[2]]);
  my $d2 = DotProduct([$points[0],$points[1],$points[0],$points[1]]);
  my $dp = CrossProduct([$points[2],$points[0],$points[1]]) / sqrt $d2;
  if ($d1 <= 0) {
    return SegmentLength([$points[2],$points[0]]);
  } elsif ($d2 <= $d1) {
    return SegmentLength([$points[2],$points[1]]);
  } else {
    return $dp;
  }
}

sub CrossProduct {
  my $pointsref = $_[0];
  my @points = @$pointsref;
  if (@points != 3) {
    carp("Need 3 points for a cross product");
    return;
  }
  my @p1 = @{$points[0]};
  my @p2 = @{$points[1]};
  my @p3 = @{$points[2]};
  my $det_p2p3 = &Determinant($p2[0], $p2[1], $p3[0], $p3[1]);
  my $det_p1p3 = &Determinant($p1[0], $p1[1], $p3[0], $p3[1]);
  my $det_p1p2 = &Determinant($p1[0], $p1[1], $p2[0], $p2[1]);
  return ($det_p2p3-$det_p1p3+$det_p1p2);
}

sub Determinant {
  my ($x1,$y1,$x2,$y2) = @_;
  return ($x1*$y2 - $x2*$y1);
}
